#include "MY_INC/APP/APP_Init.h"
extern System g_acLoadSystem; // 全局变量定义，确保所有未显式初始化的成员为零/NUL

void System_Init(void)
{
    Cordic_SinCos_Init();

    FLASH->ACR |= (1 << 8);  // 开启预取指令缓存
    FLASH->ACR |= (1 << 9);  // 开启Cache缓存，两句一起写才有效
    FLASH->ACR |= (1 << 10); // 开启Cache缓存

    ADC_Init();
    DAC_Init();

    Uart1_Init(); // 串口初始化
    Uart2_Init(); // 串口初始化

    Loop_Init(&g_acLoadSystem.controller[0].param); // 初始化环路
    Loop_Init(&g_acLoadSystem.controller[1].param); // 初始化环路
    g_acLoadSystem.controller[0].loop_sm.init_flag = Activated;
    g_acLoadSystem.controller[1].loop_sm.init_flag = Activated;
    // 关闭PWM总闸
    LL_GPIO_ResetOutputPin((GPIO_TypeDef *)g_acLoadSystem.controller[0].pwm.pwm_enable_gpio_port,
                           g_acLoadSystem.controller[0].pwm.pwm_enable_pin);
    LL_GPIO_ResetOutputPin((GPIO_TypeDef *)g_acLoadSystem.controller[1].pwm.pwm_enable_gpio_port,
                           g_acLoadSystem.controller[1].pwm.pwm_enable_pin);
    g_acLoadSystem.controller[0].pwm.pwm_gpio_flag = Nonactivated; // 设置PWM使能标志位
    g_acLoadSystem.controller[1].pwm.pwm_gpio_flag = Nonactivated;

    LL_HRTIM_TIM_CounterEnable(
        HRTIM1, LL_HRTIM_TIMER_MASTER | g_acLoadSystem.controller[1].pwm.timer[0] |
                    g_acLoadSystem.controller[1].pwm.timer[1] | g_acLoadSystem.controller[0].pwm.timer[0] |
                    g_acLoadSystem.controller[0].pwm.timer[1]); // 使能HRTIME的Timer计数器开始计数

//    LL_HRTIM_EnableIT_CMP1(HRTIM1, LL_HRTIM_TIMER_MASTER); // 使能HRTIME的Timer_Master更新事件中断

    LL_HRTIM_TIM_SetCompare1(HRTIM1, g_acLoadSystem.controller[1].pwm.timer[0], 0);
    LL_HRTIM_TIM_SetCompare1(HRTIM1, g_acLoadSystem.controller[1].pwm.timer[1], 0);
    LL_HRTIM_TIM_SetCompare1(HRTIM1, g_acLoadSystem.controller[0].pwm.timer[0], 0);
    LL_HRTIM_TIM_SetCompare1(HRTIM1, g_acLoadSystem.controller[0].pwm.timer[1], 0);
}

/***PR环路****/

PR_Diff_Typedef PR_50Hz_Inv_I = {0};
PR_Diff_Typedef PR_150Hz_Inv_I = {0};
PR_Diff_Typedef PR_250Hz_Inv_I = {0};

PR_Diff_Typedef PR_50Hz_Rec_I = {0};
PR_Diff_Typedef PR_150Hz_Rec_I = {0};
PR_Diff_Typedef PR_250Hz_Rec_I = {0};
PR_Diff_Typedef PR_50Hz_Rec_V = {0};
PR_Diff_Typedef PR_150Hz_Rec_V = {0};
PR_Diff_Typedef PR_250Hz_Rec_V = {0};

PR_Diff_Typedef PR_V_Rec;

#define PR_WC (2.0f * _PI * 0.1f)  // PR环路谐振点带宽
#define PR_WO (2.0f * _PI * 50.0f) // PR环路谐振点带宽

volatile static float PR_Kp_i_Base = 1.0f;             // PR电流环Kp
volatile static float PR_Kr_i_Base = 25.0f;            // PR电流环Kr
volatile static float PR_Kr_i_Harmonic = 25.0f; // PR电流环Kr
volatile static float PR_Kp_v_Base = 1.0f;             // PR电压环Kp
volatile static float PR_Kr_v_Base = 25.0f;            // PR电压环Kr
volatile static float PR_Kr_v_Harmonic = 25.0f;            // PR电压环Kr
/***锁相环****/
PLL_Typedef           PLL_Inv;
PLL_Typedef           PLL_Rec;
volatile static float PLL_Kp = 9.85f;  // PI环Kp
volatile static float PLL_Ki = 0.025f; // PI环Ki

/*****单相变换*****/
TOGI_Typedef TOGI_VGrid_Inv;  // TOGI变换
TOGI_Typedef TOGI_VGrid_Rec;  // TOGI变换
TOGI_Typedef TOGI_IL_Rec;     // TOGI变换
static float TOGI_K = 1.414f; // TOGI增益

/******变步长******/
VariableStep_Typedef PR_IREF_Inv; // 电流参考变步长补偿
VariableStep_Typedef PR_VREF_Inv; // 电压参考变步长补偿
VariableStep_Typedef PR_IREF_Rec; // 电流参考变步长补偿
VariableStep_Typedef PR_VREF_Rec; // 电流参考变步长补偿

/*功率环路*/
VariableStep_Typedef   Power_Pe_Rec;                  // 电流参考变步长补偿
VariableStep_Typedef   Power_Qe_Rec;                  // 电流参考变步长补偿
AntiWindupPID__Typedef PI_Power_Pe;                   // 电压PI
AntiWindupPID__Typedef PI_Power_Qe;                   // 电压PI
PQ_Typedef PQ_Rec;
float                  PI_Kp_pq = 0.1f;               // 限幅
float                  PI_Ki_pq = 2.0f;            // 限幅
float                  PI_Outmin_pe = -5.0f * Sqrt_2; // 限幅
float                  PI_Outmax_pe = 5.0f * Sqrt_2;  // 限幅
float                  PI_Outmin_qe = -1.0f * Sqrt_2; // 限幅
float                  PI_Outmax_qe = 1.0f * Sqrt_2;  // 限幅




AntiWindupPID__Typedef PI_Udc_Inv;         // 电压PI
float                  PI_Kp_v = 0.2f;     // 电压PI
float                  PI_Ki_v = 2.0f;     // 电压PI
float                  PI_Outmin = -8.0f; // 电压PI
float                  PI_Outmax = 8.0f;  // 电压PI

FilterTrap_Typedef FilterTrap_Udc;
SignalAnalysisr_Typedef SignalAnalysisr_Inv;
SignalAnalysisr_Typedef SignalAnalysisr_Rec_v;
SignalAnalysisr_Typedef SignalAnalysisr_Rec_i;


void       Loop_Init(LoopParameter *loopparam)
{
    TOGI_Init(loopparam->togi_v, TOGI_K, loopparam->togi_v_ts);
    TOGI_Init(loopparam->togi_i, TOGI_K, loopparam->togi_i_ts);
    PLL_Init(loopparam->pll, PLL_Kp, PLL_Ki, 2.0f * 50.0f * _PI, TS_20US); // 初始化PLL
    PR_Diff_Init((loopparam->pr_i[0]), PR_Kp_i_Base, PR_Kr_i_Base, PR_WO, PR_WC, loopparam->pr_ts, 20.0f,
                 -24.0f); // 初始化PR环路
    PR_Diff_Init((loopparam->pr_i[1]), 0, PR_Kr_i_Harmonic, 3.0f * PR_WO, PR_WC, loopparam->pr_ts, 10.0f,
                 -10.0f); // 初始化PR环路
    PR_Diff_Init((loopparam->pr_i[2]), 0, PR_Kr_i_Harmonic, 5.0f * PR_WO, PR_WC, loopparam->pr_ts, 10.0f,
                 -10.0f); // 初始化PR环路

    PR_Diff_Init((loopparam->pr_v[0]), PR_Kp_v_Base, PR_Kr_v_Base, PR_WO, PR_WC, loopparam->pr_ts, 5.5f,
                 -5.5f); // 初始化PR环路
    PR_Diff_Init((loopparam->pr_v[1]), 0, PR_Kr_v_Harmonic, 3.0f * PR_WO, PR_WC, loopparam->pr_ts, 1.0f,
                 -1.0f); // 初始化PR环路
    PR_Diff_Init((loopparam->pr_v[2]), 0, PR_Kr_v_Harmonic, 5.0f * PR_WO, PR_WC, loopparam->pr_ts, 1.0f,
                 -1.0f); // 初始化PR环路

    AntiWindup_PID_Init(loopparam->pi_udc, PI_Kp_v, PI_Ki_v, 0.0f, loopparam->pid_ts, PI_Outmin, PI_Outmax);
    AntiWindup_PID_Init(loopparam->pi_pe, PI_Kp_pq, PI_Ki_pq, 0.0f, loopparam->pid_ts, PI_Outmin_pe, PI_Outmax_pe);
    AntiWindup_PID_Init(loopparam->pi_qe, PI_Kp_pq, PI_Ki_pq, 0.0f, loopparam->pid_ts, PI_Outmax_qe, PI_Outmax_qe);

    VariableStep_Init(loopparam->iref, 1.0f / loopparam->variableStep_ts / 1000.0f, 500.0f, 0.0f);    // 电流参考初始化
    VariableStep_Init(loopparam->udcref, 1.0f / loopparam->variableStep_ts / 1000.0f, 1000.0f, 40.0f); // 电压参考初始化
    VariableStep_Init(loopparam->vref, 1.0f / loopparam->variableStep_ts / 1000.0f, 1000.0f, 0.0f);   // 电压参考初始化
    VariableStep_Init(loopparam->peref, 1.0f / loopparam->variableStep_ts / 1000.0f, 1000.0f, 0.0f);  // 电压参考初始化
    VariableStep_Init(loopparam->qeref, 1.0f / loopparam->variableStep_ts / 1000.0f, 1000.0f, 0.0f);  // 电压参考初始化

    Filter_Trap_Init(loopparam->trap_udc, loopparam->filtertrap_ts, 2.0f * 100.0f * _PI, 1e-5f, 1.0f);
    SignalAnalysisr_Init(loopparam->SignalAnalysisr_v, 1.0f / loopparam->signalanalysisr_ts, 0.0f,
                         980.0f / (loopparam->signalanalysisr_ts / TS_20US),
                         1020.0f / (loopparam->signalanalysisr_ts / TS_20US)); // 信号分析器初始化
    SignalAnalysisr_Init(loopparam->SignalAnalysisr_i, 1.0f / loopparam->signalanalysisr_ts, 0.0f,
                         980.0f / (loopparam->signalanalysisr_ts / TS_20US),
                         1020.0f / (loopparam->signalanalysisr_ts / TS_20US));
    PQ_Process_Init(loopparam->pq, 0.908612072f);
}
void Loop_Reset(LoopParameter *loopparam)
{
    TOGI_Reset(loopparam->togi_v);
    TOGI_Reset(loopparam->togi_i);
    PLL_Reset(loopparam->pll);
    PR_Diff_Reset(loopparam->pr_i[0]);
    PR_Diff_Reset(loopparam->pr_i[1]);
    PR_Diff_Reset(loopparam->pr_i[2]);
    AntiWindup_PID_Reset(loopparam->pi_udc);
    VariableStep_Reset(loopparam->udcref, 40.0f);
    VariableStep_Reset(loopparam->iref, 0.0f);
    VariableStep_Reset(loopparam->iref, 0.0f);
    Filter_Trap_Reset(loopparam->trap_udc);
    SignalAnalysisr_Reset(loopparam->SignalAnalysisr_v);
    SignalAnalysisr_Reset(loopparam->SignalAnalysisr_i);
}
